#include <iostream>
#include <math.h> 

#include <ros/ros.h>

#include <inspectrobot/serial_port.h>
#include <geometry_msgs/Twist.h>

// #include <vector>
// #include <serial/serial.h>
// #include <std_msgs/String.h>
// #include <std_msgs/Empty.h>
#include <curses.h>

using namespace std;


serial::Serial ser;
// 速度控制消息的回调函数
void cmdCallback(const geometry_msgs::Twist& msg)
{
    RobotV_  = int(msg.linear.x);//转每分
    theta_deg = msg.angular.z / M_PI *180;//转为角度方便取于，不然弧度取余误差太大
    theta_deg_rev = int(theta_deg)%360;

    //将左右转角限制在小于等于45度范围内
    if(theta_deg_rev >= 180){
        theta_deg_rev = theta_deg_rev -360;
        if(theta_deg_rev <= (-45)){
            theta_deg_rev = -45;
        }
    }else if(theta_deg_rev < 180 && theta_deg_rev >0){
        if(theta_deg_rev >= (45)){
            theta_deg_rev = 45;
        }
    }else if(theta_deg_rev < 0 && theta_deg_rev > (-180)){
        if(theta_deg_rev <= (-45)){
            theta_deg_rev = -45;
        }
    }else if(theta_deg_rev <= (-180) && theta_deg_rev >(-360) ){
        theta_deg_rev = theta_deg_rev +360;
        if(theta_deg_rev >= (45)){
            theta_deg_rev = 45;
        }
    }else{
        Flag_RobotYawRate_site = 1;
    }
    theta_rad = theta_deg * M_PI/180;

    if(Flag_RobotYawRate_site == 1){
        RobotYawRate_ = 0;
        Flag_RobotYawRate_site = 0;
    }else{
        RobotYawRate_ = 460/sin(msg.angular.z);//rad/s变成转弯半径mm
    }
    
    cout << "接收速度为" << RobotV_ << endl;
    cout << "接收转弯半径为" << RobotYawRate_ << endl;
}


//读取回调函数——读入new_buffer中


int main(int argc, char* argv[])
{
    setlocale(LC_ALL,"");
    //初始化ROS节点
    ros::init(argc, argv, "InspectRobot_move");
    ros::NodeHandle nh;
    
    ros::Subscriber sub = nh.subscribe("cmd_vel", 50, cmdCallback);//接收半径速度命令

    //串口初始化
    if(serial_init(ser) == -1){
        return -1;
    }
    
    //初始化robot,发送使能    
    if(!robot_init(ser)){
        ROS_ERROR("InspectRobot 初始化失败");
    }
    else {
        ROS_INFO("InspectRobot 初始化成功.");
    }

    //循环运行
    ros::Rate loop_rate(2);//2Hz
    //while ( read > 0 && init>0) 
    while (ros::ok() ) 
    
    {

        // 机器人控制
        robot_move(ser);

        loop_rate.sleep();
        ros::spinOnce();
    }
        //关闭串口
    ser.close();
 
    return 0;
}





